/*=+--+=#=+--     Unmanned Aerial System Management Software      --+=#=+--+=#*\
|          Copyright (C) 2011 Regents of the University of Colorado.           |
|                             All Rights Reserved.                             |

     This program is free software: you can redistribute it and/or modify
     it under the terms of the GNU General Public License version 2 as
     published by the Free Software Foundation.

     This program is distributed in the hope that it will be useful,
     but WITHOUT ANY WARRANTY; without even the implied warranty of
     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
     GNU General Public License for more details.

     You should have received a copy of the GNU General Public License
     along with this program.  If not, see <http://www.gnu.org/licenses/>.

            Jack Elston                       Cory Dixon                        
|           elstonj@colorado.edu              dixonc@colorado.edu              |
|                                                                              |
\*=+--+=#=+--                 --+=#=+--+=#=+--                    --+=#=+--+=#*/

/*!
  \file stargazer.cxx
  \brief class implemenation for the Hagisonic Stargazer

  \author Maciej Stachura and his wingman Jack Elston
  $Date: 2005/02/12 00:20:45 $
*/

#include <iostream>
#include <math.h>

#include "stargazer.h"
#include "color.h"
#include "GPS.h"

/*<-------Defines--------->*/
/*<------End Defines------>*/

/*<---External Global Variables---->*/
extern bool verbose;

Matrix f_roomba(Matrix, FiltData_t);
Matrix g_roomba(Matrix, FiltData_t);
Matrix g_od(Matrix, FiltData_t);

/*<-Global Variables-->*/

StargazerParser::StargazerParser(String name) : ThreadedPipe(name)
{
	ang_corr = 0.0;
  od_start = 1;
  good_star = 1;
  init_od = 0;
  // Initialize the GPS Message
	gps_r.sog = 0;
	gps_r.track = 0;
	gps_r.pos.alt = 0; gps_r.pos.lat = 0; gps_r.pos.lon = 0; 
	gps_r.utc.hours = 0; gps_r.utc.minutes = 0; gps_r.utc.seconds = 0;
	gps_r.accuracy.sats = 0; gps_r.accuracy.quality = 0; gps_r.accuracy.pdop = 0;

	gps_f.sog = 0;
	gps_f.track = 0;
	gps_f.pos.alt = 0; gps_f.pos.lat = 0; gps_f.pos.lon = 0; 
	gps_f.utc.hours = 0; gps_f.utc.minutes = 0; gps_f.utc.seconds = 0;
	gps_f.accuracy.sats = 0; gps_f.accuracy.quality = 0; gps_f.accuracy.pdop = 0;

  // Pseudolite Locations
  p_pos[0].ID = 9206; p_pos[0].x = 3.7036; p_pos[0].y = 0.8576; p_pos[0].angle = 3.8;
  p_pos[1].ID = 9234; p_pos[1].x = 3.7416; p_pos[1].y = 2.3474; p_pos[1].angle = -6.7;
  p_pos[2].ID = 9252; p_pos[2].x = 3.7406; p_pos[2].y = 4.0786; p_pos[2].angle = -2.1;
  p_pos[3].ID = 9238; p_pos[3].x = 2.3694; p_pos[3].y = 3.8776; p_pos[3].angle = -1.4;
  p_pos[4].ID = 9216; p_pos[4].x = 2.3654; p_pos[4].y = 0.8796; p_pos[4].angle = 2;
  p_pos[5].ID = 9202; p_pos[5].x = 0.9322; p_pos[5].y = 0.8716; p_pos[5].angle = 3.5;
  p_pos[6].ID = 9218; p_pos[6].x = 1.0212; p_pos[6].y = 2.4088; p_pos[6].angle = -1;
  p_pos[7].ID = 9236; p_pos[7].x = 0.9122; p_pos[7].y = 4.0586; p_pos[7].angle = 5.5;
  p_pos[8].ID = 9232; p_pos[8].x = 2.3634; p_pos[8].y = 2.3368; p_pos[8].angle = 0.45;


  R_od.SetSize(2,2);
  R_od.Null();
  R_od(0,0) = 0.00001;
  R_od(1,1) = pow(0.1*M_PI/180.0,2);

  R_star.SetSize(3,3);
  R_star.Null();
  R_star(0,0) = pow(0.09,2);
  R_star(1,1) = pow(0.09,2);
  R_star(2,2) = pow(M_PI/180, 2);
  

  // Odometry UKF Parameters
  filt.param.sig_edit = 0;
  filt.param.alpha = 2;
  // Process Noise
  filt.param.Q.SetSize(6,6);
  filt.param.Q.Null();
  filt.param.Q(0,0) = 0.01;
  filt.param.Q(1,1) = 0.01;
  filt.param.Q(2,2) = 0.01;
  filt.param.Q(3,3) = 0.01;
  filt.param.Q(4,4) = 5*M_PI/180;
  filt.param.Q(5,5) = 15*M_PI/1800;
  // Measurement Noise
  filt.param.R = R_star;
  // Covariance
  filt.P.SetSize(6,6);
  filt.P.Null();
  filt.P(0,0) = 0.01;
  filt.P(1,1) = 0.01;
  filt.P(2,2) = 0.01;
  filt.P(3,3) = 0.01;
  filt.P(4,4) = 5*M_PI/180;
  filt.P(5,5) = M_PI/1800;
  // State and Measurement Functions
  filt.f = &f_roomba;
  filt.g = &g_roomba;
  z_od.SetSize(2,1);
  z_star.SetSize(3,1);
  z_last.SetSize(3,1);

	buf_ptr = 0;

	log_name = generteNumberedFile( "/tmp/", "stargazer_");
}

bool StargazerParser::init(ShmStruct * shmStructPtr)
{
	ThreadedPipe::init(shmStructPtr);
	last_telem_tx.stamp();

	Client * me = shmPtr->clients.getClient(0);
	subscribe(CMD_ROOMBA_STATUS, me, true, INBOARD_TX);
  
	return true;
}

void StargazerParser::update() 
{
	int val, maxFD;
	fd_set readFDs, writeFDs;                   // fd sets
	struct timeval timeout;
	bool good_read = 0;

	FD_ZERO(&readFDs);	                    		// zero read fd set
	FD_ZERO(&writeFDs);	                    		// zero write fd set
	FD_SET(fifo_FDs[OUTBOARD_RX],&readFDs);			// add fifo
	FD_SET(fifo_FDs[INBOARD_RX],&readFDs);			// add fifo
	FD_SET(fifo_FDs[INBOARD_TX],&writeFDs);			// add fifo
	FD_SET(fifo_FDs[OUTBOARD_TX],&writeFDs);		// add fifo

	maxFD = fifo_FDs[OUTBOARD_RX];
	if(fifo_FDs[INBOARD_RX] > maxFD) maxFD = fifo_FDs[INBOARD_RX];
	if(fifo_FDs[INBOARD_TX] > maxFD) maxFD = fifo_FDs[INBOARD_TX];
	if(fifo_FDs[OUTBOARD_TX] > maxFD) maxFD = fifo_FDs[OUTBOARD_TX];

	timeout.tv_sec = TIME_OUT_SEC; timeout.tv_usec = TIME_OUT_USEC;

	if( (val = select(maxFD + 1, &readFDs, &writeFDs,NULL,&timeout)) <= 0) {
		if( val != 0 ) {
			if (errno != EINTR) {
				perror("StargazerParser::update() select");
				execRate = -1;
			} else
				cout << getName() << "StargazerParser::update() - select interrupted" << endl;
		} else cout << getName() << "::StargazerParser::update() - select timed out" << endl;
	}
	else {
    if(FD_ISSET(fifo_FDs[INBOARD_RX], &readFDs)) {
      int n = 0;
      n=readFifo(INBOARD_RX);
      if(n>0) {
        Command cmd((unsigned char *)fifo_msg.data);
        if(!cmd.isValid())
          cout << getName() << "::update() - invalid command"<< endl << cmd ;
				else
					parseInternalData(cmd);
      }
    }
		if(FD_ISSET(fifo_FDs[OUTBOARD_RX], &readFDs))
		{
			int n = 0;
			n=readFifo(OUTBOARD_RX);
			if(n > 0) {
				// process read commands
				int start_ptr = 0;
				if(buf_ptr == 0) {
					for(; start_ptr<n; start_ptr++){
						if ((fifo_msg.data[start_ptr] == '~') && (fifo_msg.data[start_ptr+1] == '^'))
							break;
					}
				}
				if(buf_ptr + n > PSEUDO_MEA_BUF_SIZE)
					buf_ptr = 0;
				else {
					memcpy(&(buf[buf_ptr]),&(fifo_msg.data[start_ptr]),n-start_ptr);
					buf_ptr += n-start_ptr;

					int size = 0;
					bool full_packet = false; 
					for(; size < buf_ptr; size++) {
						if(buf[size] == '`') {
							buf[size] = 0;
							full_packet = true;
							break;
						}
					}

					if(full_packet) {
            good_read = StargazerParser::process_data(buf); // True if you got a proper measurement
            if (good_read && filt.x.length() > 0)
            {
              clock_gettime(CLOCK_MONOTONIC, &t);
              tf = (float)t.tv_sec + (float)t.tv_nsec*1e-9;

              Matrix z_star_old = z_star;
              z_star(0,0) = gps_r.pos.lon;
              z_star(1,0) = gps_r.pos.lat;
              while (filt.x(4,0)*180/M_PI - (gps_r.track + ang_corr*180/M_PI) >  300) ang_corr += 2*M_PI;
              while (filt.x(4,0)*180/M_PI - (gps_r.track + ang_corr*180/M_PI) < -300) ang_corr -= 2*M_PI;
              z_star(2,0) = gps_r.track*M_PI/180 + ang_corr;

              // Time update
              filt.param.R = R_star;
              filt.g = &g_roomba;
              filt.param.dataF[0] = tf - ti;
              filt.time_update();
              ti = tf;

              logfile=fopen(log_name.c_str(),"a");
							if(logfile != NULL) {
								fprintf(logfile,"1 %g %g %g %g %g %g %g %d %d\n",
										tf,
										gps_r.pos.lon,
										gps_r.pos.lat,
										gps_r.track,
										filt.x(0,0),
										filt.x(1,0),
										filt.x(4,0)*180/M_PI,
										0,
										0);
								fflush(logfile);
								fclose(logfile);
							}

              
              if ( sqrt( pow(z_star(0,0) - z_star_old(0,0),2) + pow(z_star(1,0) - z_star_old(1,0),2)) > BAD_MEAS || 
                  sqrt( pow(z_star(0,0) - z_last(0,0),2) + pow(z_star(1,0) - z_last(1,0),2) ) > VEL_EST*(ti - t_last) )
              {
                // Bad Stargazer Measurement, ignore and send to odometry UKF
                od_start = 1;
                good_star = 0;
              }else
              {
                od_start = 1;
                good_star = 1;
                filt.meas_update(z_star);
                t_last = ti;
                z_last = z_star;
							  setSharedMem();
              }

            }else if (good_read){   // Need to initialize the state vector to the first measurement

              // Initialize the state to the first measurement and zeros
              filt.x.SetSize(6,1);
              filt.x.Null();
              filt.x(0,0) = gps_r.pos.lon;
              filt.x(1,0) = gps_r.pos.lat;
              filt.x(4,0) = gps_r.track*M_PI/180;

              z_star(0,0) = gps_r.pos.lon;
              z_star(1,0) = gps_r.pos.lat;
              z_star(2,0) = gps_r.track*M_PI/180 + ang_corr;

              z_last = z_star;

              // Start the counter
              clock_gettime(CLOCK_MONOTONIC, &t);
              ti = (float)t.tv_sec + (float)t.tv_nsec*1e-9;
              t_last = ti;
            }
						if(buf_ptr-(size+1) > 0) { 
							memcpy(buf,&(buf[size+1]),buf_ptr-(size+1));
							buf_ptr = buf_ptr-(size+1);
						}else buf_ptr = 0;
					}
				}
			}
		}
		if(FD_ISSET(fifo_FDs[INBOARD_TX], &writeFDs) && filt.x.length() > 0) {
			// process write commands

      // First propagate the state to the current time.
			if((currentStamp - last_telem_tx ) > (SG_TELEMETRY_PERIOD * SEC2MICRO) ) {
				last_telem_tx.stamp();

				gps_f.pos.lon = filt.x(0,0);
				gps_f.pos.lat = filt.x(1,0);
				gps_f.track = filt.x(4,0)*180/M_PI;
        while ( gps_f.track < 0 ) gps_f.track += 360;
        while ( gps_f.track >= 360 ) gps_f.track -= 360;


				GPSTelem_t * dataPtr        = (GPSTelem_t *)CMD_DATAPTR(fifo_msg.data);
				CMD_CMD((fifo_msg.data))    = CMD_GPS_TELEM;
				CMD_NUM((fifo_msg.data))    = sizeof(GPSTelem_t);
				dataPtr->utc.hours          = gps_f.utc.hours; 
				dataPtr->utc.minutes        = gps_f.utc.minutes;
				dataPtr->utc.seconds        = gps_f.utc.seconds;
				dataPtr->pos.lat            = gps_f.pos.lat+0.1;
				dataPtr->pos.lon            = gps_f.pos.lon;
				dataPtr->pos.alt            = gps_f.pos.alt;
				dataPtr->track              = gps_f.track;
				dataPtr->sog                = gps_f.sog;
				dataPtr->accuracy.sats      = gps_f.accuracy.sats;
				dataPtr->accuracy.quality   = gps_f.accuracy.quality;
				dataPtr->accuracy.pdop      = gps_f.accuracy.pdop;
				CMD_CSUM(fifo_msg.data)     = 0;
				CMD_CSUM(fifo_msg.data)     = C_calcCheckSum((uint8_t*)fifo_msg.data);

				fifo_msg.size = CMD_SIZE(fifo_msg.data);
				fifo_msg.port = 0;

				Client * a_client;
				int i=0;
				while((a_client = shmPtr->clients.getSubscriptionsTo(CMD_GPS_TELEM,i)) != NULL)
				{
					Client * me = shmPtr->clients.getClient(0);
					fifo_msg.src.s_addr  = shmPtr->clients.getAddr(me, a_client).s_addr;
					fifo_msg.dest.s_addr = shmPtr->clients.getAddr(a_client).s_addr;
					writeFifo(INBOARD_TX);
					i++;
				}

			}
		}
	}
}

void StargazerParser::setSharedMem(void) {
	gps_f.pos.lon = filt.x(0,0);
	gps_f.pos.lat = filt.x(1,0);
	gps_f.track = filt.x(4,0)*180/M_PI;
  while ( gps_f.track < 0 ) gps_f.track += 360;
  while ( gps_f.track >= 360 ) gps_f.track -= 360;

	Client * me = shmPtr->clients.getClient(0);
	if(me != NULL) {
		mutex_lock();
		me->telemetry.gps.pos.lat = gps_f.pos.lat+0.1;
		me->telemetry.gps.pos.lon = gps_f.pos.lon;
		me->telemetry.gps.track = gps_f.track;
		mutex_unlock();
	}
}

void StargazerParser::parseInternalData(const Command &cmd)
{
  char * dataPtr = (char *)(fifo_msg.data + CMD_HEADER_SIZE);

  switch(cmd.cmd() ) {

    case CMD_ROOMBA_STATUS: {
		  memcpy(&roomba_sensors, dataPtr, sizeof(SCISensorPacket_t));

      // If filter is not initialized, or you are getting good stargazer, then no odometry updates.
			if (filt.x.length() == 0) return;

      if (od_start){ // Initialize Odometry filter
        filt_t = filt;
        filt_t.param.dataF[1] = filt_t.x(0,0);
		    filt_t.param.dataF[2] = filt_t.x(1,0);
		    filt_t.param.dataF[3] = filt_t.x(4,0);
        od_start = 0;
        good_star = 0;
        init_od = 1;
      }else if (!good_star)
      {
        if (init_od) {filt = filt_t; init_od = 0;}

        z_od(0,0) = ((Matrix_t)(roomba_sensors.group_2.distance))/1000.0;
        z_od(1,0) = -((Matrix_t)(roomba_sensors.group_2.angle))*M_PI/180;
	
        filt.param.dataF[0] = roomba_sensors.time-ti;
        ti = roomba_sensors.time;

        filt.param.R = R_od;
        filt.g = &g_od;
 
        if (z_od(0,0) == 0){
          filt.x(2,0) = 0.0;
          filt.x(3,0) = 0.0;
        }

        filt.time_update();
        filt.meas_update(z_od);

        filt.param.dataF[1] = filt.x(0,0);
		    filt.param.dataF[2] = filt.x(1,0);
		    filt.param.dataF[3] = filt.x(4,0);

			  setSharedMem();

      }

      logfile=fopen(log_name.c_str(),"a");
			if(logfile != NULL) {
				fprintf(logfile,"2 %g %g %g %g %g %g %g %g %g\n",
						roomba_sensors.time,
						0.0,
						0.0,
						0.0,
						filt.x(0,0),
						filt.x(1,0),
						filt.x(4,0)*180/M_PI,
						((Matrix_t)(roomba_sensors.group_2.distance))/1000.0,
						-((Matrix_t)(roomba_sensors.group_2.angle))*M_PI/180);
				fflush(logfile);
				fclose(logfile);
			}


        break;
      }
    default:
      break;
  }
}


bool StargazerParser::process_data(uint8_t * msg){
	char mode;
  float angle, x, y;
  int ID;
	const char * str;
	str = (const char *) msg;

	sscanf(str, "~^%c%i|%f|%f|%f", &mode, &ID, &angle, &x, &y );
	x = x/100; y = y/100;
	// Search through the valid pseudolites.
	for (int i=0;i<SIZE_PDATA;i++){
		if (ID == p_pos[i].ID){
			gps_r.track = angle + p_pos[i].angle;
      while ( gps_r.track < 0 ) gps_r.track += 360;
      while ( gps_r.track >= 360 ) gps_r.track -= 360;
      gps_r.pos.lon = x*cos(p_pos[i].angle*M_PI/180) + y*sin(p_pos[i].angle*M_PI/180) + p_pos[i].x;
      gps_r.pos.lat = y*cos(p_pos[i].angle*M_PI/180) - x*sin(p_pos[i].angle*M_PI/180) + p_pos[i].y;
	    return 1;}
	}
	if(verbose)
  	cout << "Invalid Pseudolite: " << ID << ".  " << str << endl;
	return 0; // Not a valid pseudolite.
}


// Roomba state transition function.
Matrix f_roomba(Matrix x, FiltData_t param){
  Matrix out(6,1);

  // Update the angle (assume constant angular velocti)
  out(4,0)  = x(4,0) + param.dataF[0]*x(5,0);
  out(5,0)  = x(5,0);

  // Update the velocity based on constant magnitude and changing heading
  out(2,0)  = sqrt( pow(x(2,0),2) + pow(x(3,0),2) ) * sin(out(4,0)); 
  out(3,0)  = sqrt( pow(x(2,0),2) + pow(x(3,0),2) ) * cos(out(4,0));

  // Update the position based on the average of velocity across the sampling time
  out(0,0)  = x(0,0) + (x(2,0) + out(2,0))/2 * param.dataF[0];
  out(1,0)  = x(1,0) + (x(3,0) + out(3,0))/2 * param.dataF[0];

  return out;
}
// Stargazer Measurement funciton.
Matrix g_roomba(Matrix x, FiltData_t param){
  Matrix out(3,1);
  out(0,0) = x(0,0);
  out(1,0) = x(1,0);
  out(2,0) = x(4,0);
  return out;
}
// Odometry Measurement funciton.
Matrix g_od(Matrix x, FiltData_t param){
  Matrix out(2,1);
  // Sensor measures [dx,d_angle]
  out(0,0) = sqrt( pow(param.dataF[1]-x(0,0), 2) + pow(param.dataF[2]-x(1,0), 2) );
  out(1,0) = x(4,0) - param.dataF[3];

  if (-tan(param.dataF[3])*x(0,0) + x(1,0) + param.dataF[2] - tan(param.dataF[3])*param.dataF[1] < 0)
    out(0,0) *= -1;


  return out;
}


